# !/usr/bin/env python3
# -*- coding: utf-8 -*-
# @Time    : 2025/07/21 16:00
# @Author  : Yida Hao
# @File    : main.py

import rclpy
from rclpy.executors import MultiThreadedExecutor
from hardware_control_py.get_joint_states import GetJointStateService
from hardware_control_py.get_camera import GetCameraService
from hardware_control_py.set_joint_cmd import SetJointCommandService

"""
Main entry point for the hardware control module.
Launches all related service nodes
"""

def main(args=None):
    rclpy.init(args=args)

    joint_state_service = GetJointStateService()
    camera_service = GetCameraService()
    joint_command_service = SetJointCommandService()
    
    executor = MultiThreadedExecutor()
    executor.add_node(joint_state_service)
    executor.add_node(camera_service)
    executor.add_node(joint_command_service)

    executor.spin()

    joint_state_service.destroy_node()
    camera_service.destroy_node()
    joint_command_service.destroy_node()

    rclpy.shutdown()
